#include "pid_control.h"
#include "stdio.h"
#include "led.h"

// 读取参数

float pitch,roll,yaw;
short gyrox,gyroy,gyroz;
int encoder_left, encoder_right;
short aacx,aacy,aacz;
int pwm_output;

// 直立环控制器参数
float Stand_Expect_Angle = 0.5;
float Stand_Current_Angle;
float Stand_Angular_Speed;
float Stand_Kp = 180;
float Stand_Kd = 2;

// 速度环控制参数
int clear_encoder = 0;
float Speed_Kp=0.6,Speed_Ki=0.003;

// 角度环控制参数
float Turn_Kp, Turn_Kd;


// 速度环，实际速度是两轮的速度，因为两个速度都会影响快慢
int Speed_Controller(int target, int encoder_l, int encoder_r)
{
	static int err_last, err_sum;
	static float a=0.7; //滤波系数
	Speed_Ki = Speed_Kp/200;
	int err;
	err = (encoder_l + encoder_r) - target;
	// 低通滤波
	err = (1-a)*err + a*err_last;
	err_last = err; // 存储到下一次
	err_sum += err;
	err_sum=err_sum>20000?20000:(err_sum<(-20000)?(-20000):err_sum);  // 积分限幅度
	if (clear_encoder == 1)err_sum = 0, clear_encoder = 0;
	// 计算结果
	int temp = Speed_Kp*err + Speed_Ki * err_sum;
	return temp;
}


// 直立环控制器,返回值为电机控制（PWM占空比）
// 直立环会受到速度影响，所以期望值需要速度环输出补偿；
int Stand_Controller(float Expect_Angle,float Angle,float gyro_X)
{
	int temp=Stand_Kp*(Angle-Expect_Angle)+Stand_Kd*gyro_X;
	return temp;
}

// 角度环KD控制器
int Turn_Controlller(float gyro_z, int target_turn)
{
    int temp = Turn_Kp*target_turn + Turn_Kd*gyro_z;
    return temp;
}

void Pid_Updae_Params()
{
	mpu_dmp_get_data(&pitch,&roll,&yaw);
	encoder_left = Encoder_Get_Speed_Left();
	encoder_right = Encoder_Get_Speed_Right(); // 更新速度
	MPU_Get_Gyroscope(&gyrox,&gyroy,&gyroz); // 更新角速度
	MPU_Get_Accelerometer(&aacx,& aacy,&aacz); // 更新加速度
}


void Pid_Update_Speed()
{
	const float target_roll = 0;
	Pid_Updae_Params();

	float ki = 30;

	float kd = 10;

	float error = roll - target_roll;
	float output = ki * error + kd * gyrox;
    pwm_output = -output;
    if (pwm_output > 0)
	{
        TB207A1_Set_Speed(pwm_output, pwm_output);
        TB207A1_Move_Left(1);
    	TB207A1_Move_Right(1);
	}
    else
    {
    	TB207A1_Set_Speed(-pwm_output, -pwm_output);
    	TB207A1_Move_Left(0);
    	TB207A1_Move_Right(0);
    }




//	if(roll < -4)
//	{
//		TB207A1_Set_Speed(3000, 3000);
//		TB207A1_Move_Left(1);
//		TB207A1_Move_Right(1);
//		Set_LED_On();
//	}
//	else if(roll > 4)
//	{
//		TB207A1_Set_Speed(3000, 3000);
//		TB207A1_Move_Left(0);
//		TB207A1_Move_Right(0);
//		Set_LED_On();
//	}
//	else
//	{
//		TB207A1_Stop();
//		Set_LED_Off();
//	}
}

// PID 控制器，10ms运行一次
void PID_Controller()
{
	int Target_Speed = 1000;
	float Target_Angle = -3.8;
	int Target_turn = 0;

	int speed_output = Speed_Controller(Target_Speed, encoder_left, encoder_right);
	int stand_output = Stand_Controller(speed_output + Target_Angle, roll, gyrox);
	int turn_output = Turn_Controlller(gyroz,Target_turn);

	pwm_output = stand_output;

	if (pwm_output > 7200)pwm_output = 7200;
	if (pwm_output < -7200)pwm_output = -7200;
	int left_speed = pwm_output - turn_output;
	int right_speed = pwm_output + turn_output;
//	 重新配置电机速度
	TB207A1_Set_Speed(right_speed, left_speed);
	TB207A1_Move_Left(1);
	TB207A1_Move_Right(1);

}



















